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ABSTRACT 


Today's demand for a high speed, low weight and large load 
Capable manipulator has spurred the research on flexible 
manipulators. This thesis centers on an implementation of 
dynamic control on a single-link flexible arm utilizing a general 
purpose micro-computer. This research also studies the dynamic 
behavior of the control system with a brief comparison of the 
derived flexible-body-model controller to a rigid-body-model 


controller. 
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I. INTRODUCTION 


A. BACKGROUND 

The importance of robotic manipulators to society cannot 
be overemphasized either presently nor in the future. With 
this growing dependency, greater demands are placed on 
Manipulators to react quicker, weigh less and support 
greater loads. The flexible manipulator offers low power 
consumption, ease of transportability, reduced material 
requirement, lower mounting strength and rigidity 
requirement and lower overall cost [Ref 1]. 

Most work in the past have centered on rigid body 
Manipulators until the early 1970's when flexible 
manipulators began to show some promise in the space 
industry. To meet the need of a light-weight manipulator 
having greater performance capabilities, certain problems 
must be solved in order to fully utilize the flexible 
manipulator. The complexity of the flexible system makes 
modelling the system a challenge. The model must adequately 
describe the system and yet it must be simple enough to 
implement in order to design an adequate controller for the 
available hardware. 

The model was derived from the use of the Equivalent 
Rigid Link System (ERLS) first introduced by Chang [Ref 2]. 
The Equivalent Rigid Link System first divided the global 


motions into large and small motion components and then 
4 


described the kinematics of the large motion in terms of the 
ERLS and related the small motion kinematics relative to 
ERLS. Two sets of coupled, non-linear ordinary differential 
equations evolved from the use of finite element 
discretization of deformations and Lagrangian formed 
equations of motions. In the large motion equations, both 
the large and small motion variables are non-linear. In the 
small motion equations, the small motion variables are 
linear while the large motion variables remain non-linear. 

Petroka [Ref 2] experimentally validated the ERLS model 
through the simulation of a single-link flexible arm 
utilizing the Continuous System Modelling Program (CSMP) on 
the IBM 3033 mainframe computer. A hydraulic actuated 
vertical motion flexible arm was designed and built. The 
transverse displacement of the flexible manipulator was 
found using a cubic shape function. With the aid of a movie 
camera and strain gages, Petroka was able to validate the 
model due to general agreement between the simulation and 
the actual system response. 

Gannon [Ref 3] upgraded the model using a natural mode 
shape function and with the aid of strain gages, a 
potentiometer, and an accelerometer determined the tip 
location and dynamic behavior. Computer simulation was 
accomplished by the Dynamic Simulation Language (DSL) on the 


IBM 3033 mainframe computer. Data acquisition was performed 


Zz 


using a GWI Instruments MacAdios hardware and software and a 
Macintosh 512k computer. 

Park [Ref 4] used the Dynamic Simulation Language (DSL) 
on the IBM 3033 mainframe computer to design a closed loop 
controller for the flexible arm using torque as the control 
variable. Using three loading conditions, Park designed a 
non-linear, time-variant controller. However for 
implementation on a common AT micro-computer, the 


computations must be simplified. 


B. PURPOSE 

The purpose of this study is the implementation of a 
dynamic control of a single-link flexible arm and 
experimentation with various parameters to study the dynamic 
behavior of the control system. The tip position was 
determined by the outputs of a potentiometer and a strain 
gage. Data acquisition was performed using Data Translation 
high speed interface board (DT 2821-F-8DI). The board 
supports sixteen twelve bit A/D input channels and two D/A 
output channels and sixteen digital input/output channels 
with a maximum usable sampling rate of 130 kilohertz. The 
micro-computer used was the standard IBM AT. The support 
software (AT-LAB) allowed direct manipulation of the data 
acquisition board through the use of provided subroutines 
which were compatible with FORTRAN, the language chosen to 


implement the controller. 


The remainder of this thesis is organized into three 


chapters. 


ee 


Chapter II contains the theory behind the system 
model. 


Chapter III contains the design and implementation 
procedure of the controller. 


Chapter IV presents the results of the closed loop 
system compared to the predicted values of the 
Simulation and a brief comparison with a rigid- 
body-model controller. The chapter ends with 
conclusions and recommendations. 


Ii. MODEL THEORY AND PLANT DESIGN 


A. PHYSICAL PLANT 

Petroka built a one (1) meter long flexible arm to 
validate the Equivalent Rigid Link System Model. The arm 
and the control apparatus are shown in Figures 2.1 and 2.2 
respectively. The arm is flexible in the vertical plane and 
stiff in torsion and in the horizontal. The arm is 
electrohydraulically driven with a York hydraulic power 
unit, the pitch axis of a Berd-Johnson 3-axis Hyd-Ro-Wrist, 
a Moog 760-100 servovalve with its servocontroller and a 
high pressure filter assembly [Ref 2]. The basic 
construction of the arm is two thin one (1) meter parallel 
steel strips connected by thin steel strips to transverse 
steel plates. Loading is accomplished by attaching custom 
made .453 kg plates to the tip end transverse plate with 
four (4) welded studs as shown in Figure 2.3. Table 1 shows 
the geometric and mass properties of the flexible arm. 


TABLE I. ARM GEOMETRIC AND MASS PROPERTIES 


Arm length 0.9985 
Beam thickness 0.003175 
Beam Width 0.0762 
Distance between 0.05715 


each beam 

Arm mass 4.8565 

Modulus of 2QE11 

elasticity 

Density 7861.05 
(per unit volume) 
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Figure 2.1 - Flexible Link, Actuator and Filter Assembly 
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Figure 2.3 - Flexible Arm With Weights Attached 


B. EQUIVALENT RIGID LINK SYSTEM FOR A SINGLE-LINK ARM 

The ERLS separates the motion of the flexible link system 
into a large rigid body motion and a small motion 
displacement due to the arm's flexibility. The ERLS defines 
the large motion of a single-link flexible manipulator. The 
small motion is then described relative to the ERLS. Figure 


2.4 is the schematic diagram for a single flexible link. 





Figure 2.4 


L 


Inertia frame coordinate 
Inertia frame coordinate 
Local frame coordinate 
Local frame coordinate 

= Deflection angle 

Large angle 


xX 
ve 
x 
ag 
V/ 
6 
Three generalized coordinates are used to describe the 
large joint variable (86), the tip deflection V(0) and tip 
Slope 6(0). The large motion joint variable (86), is the 
angle between the ERLS and the horizontal axis of the 
inertial coordinate system. V(0) is the tip deflection 


measured from the ERLS defined position. 6(0) is the tip 
= 


slope defined in terms of the local coordinates for the 
ERLS. Homogeneous transformations are applied to a point 
along the arm to obtain the point*‘s absolute position. 
Since the displacements of each point along the arm is a 
function of location and time, the deformations must be 
discretized in terms of the generalized coordinates in order 
to take advantage of Lagrange‘s equation. Discretization 
was accomplished by the technique of the Finite Element 
Method (FEM) [Ref 2]. Once the kinematic relationships 
between the large and small motions are described, the 
kinetics are introduced to complete the derivation of the 
equations of motion. 

Due to its systematic approach, the Lagrangian dynamics 
approach is used to derive te equations of motion. The 
total kinetic energy is comprised of the individual kinetic 
energies of the link, actuator, and any applied forces. The 
total potential energy of the system is comprised of the 
elastic strain energy of the link and the potential energy 
due to gravity. Generalized forces are made up of any 
applied forces and damping forces. Through mathematical 
manipulations and simplifications, two sets of coupled non- 
linear equations are derived. One set describes the large 


motion and the other set describes the small motions. 


me 


These equations describe large motion and small motion 


respectively; 
Mag 8 + M,, V = F, + Torque C271) 
Mqe@t+M,V+K V= F, (Dey 
where 
needled. coupled effective inertia matrix for 
large motions. 
Mj, = 1x2 coupled inertia matrix of 
the small motion effect on 
large motions. 
Mig = 2x1 coupled inertia matrix of 
the large motion effect on 
the small motions. 
Wer ames Clrective Inertia matrix 
for small motions. 
K,, = 2x2 stiffness matrix for 
small motions. 
F = 1x1 load vector for large 
motions 
S) = generalized coordinate of the 
large motions. 
V = 2x1 generalized coordinates 


of the small motions. 
The assumptions applied for this research are that; 
a. Axial and torsional deformations are negligible. 
b. The tip displacements are small thereby, 
TAN! (V/L) = V/L (253) 
with 


=V (0) 


a 


The actual slope is also assumed to be negligible. 
Thus, only the tip deformation V is the only generalized 
coordinate being used for small motion calculations. Based 
on these assumptions all the coefficient matrices of the 
non-linear, coupled, second order ordinary differential 
equations are reduced to 1x1 matrices. The total motion of 


the arm tip is represented by; 


6 =@ +V/L (2.4) 
$ = 6 Sen (2.5) 
oS Cu (25Gn 


For a more detailed derivation of the equations of motions 


see [Ref 4}. 


C. SHAPE FUNCTION DERIVATION 

Modelling the beam as a continuous Euler-Bernoulli 
cantilever beam, a natural-mode shape function was used to 
present the flexural motion of the single-link flexible arm 
[Ref 4]. The transverse displacement, u(x,t}, for a single- 


link flexible arm is represented in the following forms; 


Ate 


pasty) oa C) Xalexaeetan( t) X> (3c) (a7) 
= a,{A', (cosf, x +coshf, x} + (2.3) 
{(sing,x + sinhf,x}} + 
a,{A', {cosf,x + coshf oy 
{sin8,x + sinh§,x}} 
where 
6,L=1.875104069 
B,L=4.6904091133 
Po (Siete Suan Gy (2.9) 
(cosf ; L + cosh , L) 
In this research, the shape function N is reduced to a 3x1 
matrix after having truncated the slope function. And the 


3x1 displacement vector da is 


d = ntU = ao 0 V(0) 
0 = 0 (raion 
V(x) M 
where 
Metre (ho COSsox 4 COShE x) + (Sing.x + sinhg,x) } 


+ C, {A', (cosf,x + coshf,x} + {(sinf,x +sinhf,x}}) 
C, = 28,/(4 A', B, - 4 A's B, } 


Cy = 28,/(4 A', Bo - 4 A's B, } 


D. HYDRAULIC ACTUATION 
The single-link flexible manipulator uses electro- 
hydraulic actuation to drive the plant. The applied current 


is transformed to an output terque which positions the arm. 
iyo 


The hydraulic dynamics 1s represented by the dynamics of the 


servovalve and actuator. The manufacturer of the servo- 


valve, Moog, simplified the servo-valve dynamics into a 


single equation (equation 2.11) [Ref 5]- 


Q=1*K*/P, (2 aly) 
where 
Q= Flow delivered from servovalve. 


I = Input current 


Valve sizing constant, which contributes 
to the hydraulic system damping 
Valve pressure , P,-P, 


K = 


PY 
P.= Supply pressure. 
P,= Load pressure. 

The continuity equation supplemented by the torque 


output equation yields the actuator dynamics. [Ref 6] 


Q= Dm*6 + Ctm*P, + Vt*P,/(4fe) (2.12) 

Td= nt*P,*Dn (2513) 
where 

Q = Flow delivered to the actuator. 


Dm*6= Flow causing actuator rotation. 

Be = Effective bulk modulus of the fluid. 

V, = Total compressed volume including 
actuator lines and chambers. 

V.*P,/ (4Be) = Compressibility flow. 


Ty, = Torque required to overcome inertia 


and move the load. 
ee 


Ctm*P, = Leaking flow in the actuator 


Motor displacement. 


Dm 


n, = Torque efficiency 


Pe Load pressure drop. 
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III. CONTROLLER DESIGN 


A. MODEL REFINEMENT 

The controller is designed and implemented on a micro- 
computer which had limited capabilities in both computation 
speed and power. This suggests that any simplifications or 
refinements to the proposed non-linear, time variant, 
coupled equations of motion would be beneficial to the 
successs of implementing a controller. Without the 
Simplifications more calculations would be needed to compute 
the coefficients which are used to design the controller. 
This would increase the time needed between system updates 
in order to compute and finally transmit a control signal. 
If the control signal is delayed too long the controller may 
not be able to keep up with the system. 

First, the coefficients in the equations of motion were 
evaluated by running the simulation program developed by 
Park [Ref 4]. It was discovered that over a large range of 
values for the total angle (¢#) (from ¢@ to 1 radian), that 
the coefficients could be considered either constant or to 
be a direct function of the large angle. This greatly 
Simplified the equations from being non-linear to becoming 
linear equations. And due to the constancy of the 
coefficients, the equations become time invariant. This 
Simplification suggested using the State Space Method for 


designing the controller. 
11 [8 


Beginning with the system equations 2.1 and 2.2 and 
eliminating V; 


M..? ng ON) Yor ee een? aire = Torque (3.1) 
where 


D= (May —Mog/L)/ (Min — Mpg/L) (3.2) 
Now combining terms results in a short form equation; 


Né + Fc(¢,%,V,V) = Torque (3.3) 
where 


N = Mo —D*M 


(24) 
Fc(#,%,V,V)= D*F, -F, -D*K,*V (38s) 


N was found to approximate a constant value while Fc was 


found to approximate a linear function of the total angle 
(¢), Figures 3.1 and 3.2 respectively. Equation 3.5 was 


converted using these approximations into Equation 3.6; 
N*d + E*S = Torque - F (3.6) 
with 


E,F = Constant coefficients 


Table II displays the coefficients for the two trial loading 
conditions. 
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Figure 3.1 - 0 Load Inertia Coefficient N 
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Figure 3.2 - 0 Load Non-Linear Term-Fc 


TABLE II. TRIAL MASS COEFFICIENTS 


MASS N E F 

(kg) -- |N*m*s/raqd) (N*m/rad) ‘(N*¥m) - 
0 ~-42164 =15 531599 28.4392 
Lw3593 1.2343 -17.72896 39.3924 


Equation 3.6 was then transformed into the State Space 









Variable format of 


X = A*X + BU (357)) 
Y = CkX (3.8) 
with 
U = Torque - F = The control input. 
A = 2x2 Matrix 
B = 2x1 Matrix 
X = 2xl State Variable Matrix 
C = 1x2 Matrix 


Table III has the values of the coefficient matrices for 


each trial mass. 


TABLE III. STATE SPACE CONTINUOUS TIME COEFFICIENTS 


mss; * .= |< 
0 1 0 jh 0 
36.8361 0 2.37147 
1.3593 0 1 0 1 0 
(kg) 14.3642 0 so Loz 
i ee ee 


The problem for the purposes of controller design has been 





converted to a linear, time invariant problen. 


B. SELECTION OF A SAMPLING RATE 

The selection of the proper sampling rate greatly 
influences whether a successful controller can be designed. 
Sampling iS necessary because input values are obtained by 
the computer at specified times either as functions of the 
computer clock or a function of the control program. 
Some of the considerations when selecting a sampling rate 
are; 


1. The minimum time required for the D/A and 
A/D conversions. 


2. The amount of control effort desired. 
3. The required time to complete all control 
computations 


The sampling rate must not be too low for it may give rise 
to aliasing or frequency folding. Aliasing and frequency 
folding describe the same phenomenon. This occurs if the 
sampling rate is not greater than twice the maximum system 
frequency. The frequencies which are greater than half the 
sampling frequency then "fold" upon themselves becoming 
additive in nature giving rise to an enhanced signal. If an 
arbitrarily high sample rate is chosen then the physical 
limitations imposed by the micro-computer and data 
acquisition hardware must be considered. Therefore, a 
trade-off is normally made. Shannon’s Sampling Theorem 
states that any signal whose highest frequency F, can be 


reconstructed if the sampling rate is twice Fo 
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However, for control purposes, this sampling rate is 
generally too low. Astrom and Wittenmark suggested using 
Six (6) to ten (10) times the system frequency bandwidth 
[Ref 8]. Initially, the sampling period was chosen as 0.005 
seconds of five (5) milli-seconds because it was the fastest 
rate in which all necessary conversions and calculations 
could be accomplished in one sample period. 

The importance of the sampling rate cannot be over- 
stressed. A high sampling rate requires greater control 
actions which is translated to greater component action that 
can lead to early equipment failure. A high sample rate 
also increases the chances of exciting higher order 
unmodelled system dynamics. So it is very important to 
understand the system under consideration in order to decide 
how much control is enough and the final choice of a 


sampling rate should be the result of that decision. 


C. CONTROLLER DESIGN 

The controller was designed using the State Space Design 
Method coupled with pole placement. The poles were selected 
in the continuous time S-domain and converted to the 


discrete time Z-domain using; 


pd = exp(h*p) (3.9) 
where 


pd = The Z-domain poles. 


The S-domain poles. 


> 
rf 


The sampling period. 
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T™e continuous time model Equaticn 2.7 was then 


convertec to tne Giscrete time difference Equation 3.10. 


X(h*k + h) = oc *X(h*k) + F*uU (30 0) 
where 

@ = exp(A*h) (SeL1) 

r= | exp(A*h)dt * B (3.42) 


Table IV contains the coefficient matrices for the trial 
masses. 


TABLE IV. am SPACE DISCRETE TIME MATRIX 7 | 


0.9995 0.0050 
- 0.1843 0.9995 O. aa 





1.3593 1.0002 0.0050 0 0 
(kg) 0.0718 1.0002 0.0041 
_ eS eee eee aaa 


Using Equations 3.9, 3.10 and Ackerman's Equation, a 1x2 


matrix of gains K was derived for given pole locations. 


From each set of gains, K, the control law was defined as 


U(k) = R(k) - K*X(k) (3.13) 


where 


R(k) = The reference signal. 
K = 1x2 matrix = [ Kp Kv] 


The positional gain. 


Kp 
Kv = The velocity gain. 
The reference input is composed of the dynamics of the 


system, the effects of the zero order hold, the sampling 
20 


rate and the required state. The derivation of the 
reference input follows. 

Within some finite number of time steps(k) a system will 
reach its required states if the poles and gains are 
properly chosen assuming that the system is controllable. 
The at time step (k+1), the state at that time will equal 
the state at time(k) multiplied times a time step increment 
or in other words 

zX(z) = Z(X(K+1)) (302) 

Equation 3.14 is the Z-transform representation. Taking 
the Z-transform of Equation 3.10 and replacing the left hand 
Side with the left hand side of Equation 3.14 will yield 

Z2X(Z) = aX(z) + FU(Z) (3.15) 

The next step is taking the Z-transform of Equation 3.13 
and replacing U(z) in Equation 3.15 with the right hand side 
of the transformed Equation 3.13. 

zX(z) = aX(z) + IP (RK(z) - KX(z)) (3.16) 
Combining terms and solving for X yields Equation 3.17. 

X(z) = [Z*I-(a-[ *K)]°' * T*RkK(z) (3.17) 
where 

I = The identity matrix 
The system output is represented by Equation 3.18. 
¥(Z) = C*¥X(Z) (3.18) 

The relationship between the output and the reference is 
derived by replacing X(z) in Equation 3.18 with the results 


of Equations aly. 
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Y¥(z) = C*[Z*I-(a - P*K))-1 * [*RK(z) (3/119) 
Let 

RK(Z) = (C*[Z*I - (a2 ~ P*K))"' *y, (3.20) 
or 3 

SSA ier! oer (BODN) 
with 


Y. = The required output. 
H(z) = (C*]Z*I - (a - [*K)]) (3.22) 

Now replace 

Ge) (32,3) 
or 

Y¥(zZ) - Yr = 0 (3.24) 
Equation 3.24 assumes that the states are fully known and 
the model exactly represents the Syetene In actual practice 
Equation 3.24 doesn’t equal zero but instead some small 
error. However, 1f the gains are carefully chosen and the 
model is a good approximation of the system, the error will 
be very small or could be approximated as zero. 

H(z) is called the "pulse transfer function" [Ref 8]. 

It is normally a quotient of two polynomials in z. Even if 
this function is stable and’ causal, the inverse of the 
function may have problems with causality or with stability. 
The causality is caused by the numerator having higher order 


terms than the denominator. This then requires that future 


ir.formation be known regarding the future output of the 
system to cetermine the present input. The problem with 
stability may occur if there is a pole that is 2 1. The 
problem with causality isn't seen since the output chosen is 
the final value for a given trajectory. The problem with 
stability can be alleviated by selecting a filter which 
replaces the unstable pole with stable pole. 

Once the reference signal is computed in the complex 
frequency domain, it is a simple matter to transforming back 
to the discrete time domain by taking the inverse Z- 
transform of Equation 3.20. After computing H(z) the 
plant's trajectory can be contlled by choosing an 
appropriate form of Y_., ie, constant value, linear to 
produce a ramp appearance, or a quadratic to produce a 
horseshoe effect. In this research the values of Y. was 
either a constant or linear. 

Once the control law was established, the control signal 
was determined and converted to a required torque. Since 
torque could not be transmitted directly, the equivalent 
current was generated using the inverse dynamics relations 


of the hydraulic actuator as described by Equations 2.12 and 


P = T,/n,*Dm (3.25) 
I = Q/(K*/P,) (3.26) 

Knowing the resistance allowed the computation of an 
output voltage which would produce the required current to 


actuate the actuator's torque motor. In this control 
26 


scheme, the value of current was limited to 4 mA to avoid 
saturation of the hydraulic actuator controller. The 
derived controller was then installed in the simulation 
model and tested to insure ENnaewieecould in fact controlmche 
modelled system. This procedure was repeated until an 
adequate controller was found. The values of the gains and 


reference signals are found in Table V. 


TABLE V. TABLE OF REFERENCE AND GAIN INPUTS 


ae 


0 867.2958 |847.7606 | 60.00 
13593 Zoe O mi 250.4961 | 37.00 


D. IMPLEMENTATION 












-200.81,-16.99 


The implementation involved the connection of the 
controller to the flexible arm. The full controlling system 
consisted of a 512k IBM micro-computer, a Bam-1 amplifier 
unit, a strain gage, a potentiometer, a control unit for the 
electrohydraulic actuator, and a high speed data acquisition 
system. The output from the strain gage was fed to the 
Bam-1 for amplification and then sent to the interface 
board. This arrangement provided the input for the small 
motion V/L. 

The output from the potentiometer was fed to the 
actuator control box which had a built-in filter to 
attenuate any noise on the large motion signal. This Signal 
was then fed to the interface »oard and this provided the 


Large angle input. a 


The angular velocity of the total angle was determined 

using a reduced order observer of the form 

X (k+1)=(X(k+1) -X(k))/h + (h/2)U(k) (3.27) 
with 

h = sampling period 

U(k) = control input 

The interface board was the Data Translation Acquisition 
Board DT-2821-F-8DI. The board supports sixteen digital 
input/output (I/0) channels, sixteen twelve bit analog to 
digital (A/D) input channels and two twelve bit digital to 
analog (D/A) output channels. The analog channels are 
capable of being configured as unipolar, or bipolar, single 
end or differential. The maximum useful sampling rate is 
130 kilohertz. 

The control sequence was then measured to ensure that it 
was less than the sampling rate. Three basic programs were 
used to design the controller and then to control the plant. 
The program PTOM DSL (Appendix A) was used to run 
Simulations of the system. The program FPOINT.FOR 
(Appendix B) was used to establish point to point control. 
The program TRAJ.FOR (Appendix C) was used to establish 


trajectory control. 


Ze 


IV. RESULTS AND CONCLUSIONS 


A. GENERAL COMMENTS 

As part of the research, a sampling rate, which could 
provide the requisite control yet would minimize the control 
effort without attempting to design an optimal controller,is 
to be determined. The adopted method was to use the highest 
sampling rate feasible to design the controller which would 
require the higher gains and then to adjust the sampling 
rate downward until the lowest value of the required torque 
was achieved while retaining the control accuracy. Also it 
was desired to stay within the band of the suggested 
Sampling rate of six to ten times a desired system 
frequency. The desired system frequency was between two to 
three times the arm's natural frequency to ensure a good 
response. This was accomplished in the design process by 
the choice of poles selected. It was found that at 50 Hz, 
the control effort or torque was cut approximately by 1/3 of 
the value required at 200 Hz for a zero loading condition as 
noted in Figures 4.1 and 4.2. Base on this, all further 
trials were conducted at 50 Hz. However, it was found later 
that in order to maintain control of the 1.36 kg load 
during a series of ramps and step inputs it was necessary to 
increase the sampling rate to 100 Hz. 

During the execution of the testing, it was noticed that 


the arm would begin to vibrate with increasing amplitude 
23 
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after the arm had reached its required position. After 
analysis, it was found that since the small angle signal was 
of such small magnitude (>0.1 volt), that the signal was 
dominated by noise with frequencies above 3 Hz. This 
problem was alleviated by installing a digital filter 
designed using Tustin's bilinear rule [Ref 8]. This is 
where the true power of the Equivalent Rigid Link System 
came into play. It allowed the control of the system while 
the search for the proper cutoff frequency was being 
conducted. It basically allowed the isolation of the small 
angle from the control circuitry and easily facilitated the 
monitoring of the signal using the control program. 
Initially 10 Hz was tried as the cutoff frequency, however 
noise still dominated the signal and at some point the 
amplitude would begin to grow. The arm's natural frequency 
was computed as approximately 2 Hz and a cutoff frequency of 
3 HZ was selected. An example of a dirty signal is shown in 


Figure 4.3 and a "clean" signal is shown in Figure 4.4. 


B. POINT TO POINT CONTROL 

This scenario has the arm follow a travel command from a 
starting point to a finishing point via response to a step 
input. During this scenario the effect of varying the 
Sampling rates and control gains were tested on the zero 
load case while expecting similar results at higher weights 


only possibly amplified. The selected sampling rates used 
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were 200 Hz, 100 Hz and 50 Hz along with two different sets 
of gain. The required output in all testing was one radian 
in order to remain consistant with previous research. The 
value of the gains and S-domain poles are in Table VI. Both 
sets of gains produce the same trend of the effect which 
sampling rate has on the steady state error. 


TABLE VI. GAINS 


SET KP KV POLES 
if 845.76 60.00 =—2 00.9 7.6. 2 
2 411.8 33.00 es BEE Pas als 


Figures 4.5 through 4.7 show the results generated by using 
the gains of set 1. It is seen that for the higher sampling 
rates small amplitude cycling about the final position 
occurs. It appears that while the higher modes of 
vibrations has been successfully filtered out their coupling 
with the fundamental mode is being activated at the higher 
sampling rates. However, the effect 1s even more pronounced 
at higher loads as demonstrated by Figure 4.8. It appears 
that lower frequency model coupling excitations occur at the 


loaded conditions. 


CG; TRAJECTORY CONTROL 
In trajectory control the same gains and reference 
signals were used as in the point to point control portion 


of the research. What has been added is that the inputs are 
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of the research. What has been added is that the inputs are 
a combination of ramp and step inputs. What is seen 
immediately is that for the zero loading case the simulated 
value and the actual value was in close agreement as shown 
in Figure 4.9. In both loading conditions the plant 
responded to the commands and followed the trajection with 
the 1.36 kg load enjoying a slightly better steady state 


error aS seen in Figure 4.10. 


D. THE PAYOFF OF THE FLEXIBLE-BODY-MODEL 

Previous research suggested that the flexible-body-model 
would allow higher speeds, larger loads, and smaller 
required torques for a flexible manipulator. A modest 
attempt has been made to see if these advantages are 
realized with a single-link manipulator. Using the 
flexible-body-controller and designing a controller using 
rigid body assumptions, their performance were compared in 
the areas of steady state error and required torques at 0 


and 1.36 Kg loads. The design of the rigid-body-controller 


follows. 

16 + mglsin(®@)/2 = Torque (4) 
and 

ee eats atte (4.2) 
where 

I. = The moment of inertia of the rotor. 


i 


I, = The moment of inertia of the arm. 


3S 


I, = The moment of inertia of the load. 

The controller equation for point to point control became 
Torque=I(-k, @ -k, (@-Y, ))+ (4.3) 
mglsin(@) /2 

where 


m = Mass of the arm and load. 


Gravitational constant. 


te} 
ll 


L 


Length of the arm. 

Y. = The required output. 
It is seen that the steady state errors are comparable in 
Figure 4.11 for the 9 load condition. Also notice that the 
flexible-body-model control displayed a faster settling 
time. However, there is clear evidence that the maximum 
required torque is approximately 1/3 lower for the flexible- 
body-model as seen in Figure 4.12. The comparison of the 
steady state error at the 1.36 Kg loading shows clearly that 
for the same set of poles with equivalent gains that the 
rigid-body- model is totally inadequate for that loading 
condition as shown in Figure 4.13. The comparison of 
required torque at 1.36 Kg loading again shows that the 
rigid body model requires much more torque as shown in 


Figure 4.14. 
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E. CONCLUSIONS 


The system is highly sensitive to noise. The entire system, 
arm controller and actuator must be isolated from both input and 
output noises. Some methods for accomplishing noise isolations 
are; 

1. Use shielded cabling. 

2. Install pre-filters. 

3. Measurement equipment and computer should be moved as far 

as possible from the hydraulic pump. 

The system is highly reactive to mode coupling at higher 
Sampling rates. 

As predicted by control theory steady state error decreased 
with increasing gains. 

The operating frequency of the system was between 1.5 to 4 
times the natural frequency of the arm. The bandwidth explored 
for the 0 load case was from 5.6 Hz to 9.5 Hz. The bandwidth for 
Emer. 36 Kg was 2.3 HZ. 

The results of the comparison between the rigid body mode 


controller and the flexible body model controller are; 


1. With increasing loads and at equivalent gains, the 
rigid-body-model has an increasing steady state error. 


2. The rigid-body-model required much more torque than the 
flexible-body-model without providing greater control. 


It appears that the results obtain points toward the 
results of the previous research. However, caution must be 


exercised and more comparison should be sought at greater loads. 
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F. 


RECOMMENDATIONS 


It is recommended that; 


1. 


The effects of modal coupling be studied to build a more 
robust controller. 


Due to the system’s non-linearities, the effects of 
imposing saturation limits on the arm should be studied. 


The state space model be expanded to four states with 
four feedback controls. two states representing the 
large angle position and velocity and two states 
representing the small angle position and velocity. 
This will allow direct control over the small motion 
component. 


Extend the present study of the flexible manipulators to 
include a double-link flexible arm and conduct a more 
thorough comparison between a flexible-body-model 
controller and a rigid-body-model controller. 
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APPENDIX A 


SAMPLE PROGRAM FOR SIMULATING THE SYSTEM 


APPENDIX A 


TITLE NO LOAD 
CONST GKV=2.0 ,GKP=1. 00,DEA=1. 0,RK=867. 24828 , TAU=0. 020 
Gees s ‘COPY 


SIMULATION OF SINGLE LINK FLEXIBLE MANIPULATOR DYNAMICS 


THIS PROGRAM SOLVES THE ERLS FLEXIBLE MANIPULATOR DYNAMICS FOR A 
SINGLE LINK EXPERIMENTAL ARM. THE EXPERIMENTAL ARM PARAMETERS ARE 
INPUTTED AND THE HYDRAULIC ACTUATION DYNAMICS ARE INCLUDED IN THE 
SVOEATIONS THE INPUE 1S THE CURRENT 7O THE SERVOVALVE MOUNTED ON 
THE HYDRAULIC ACTUATOR AND THE OUTPUT IS THE POSITION OF THE ARM 
TiPSIN THE GLOBAL REFERENCE SYSTEM. THE CODING CONSISTS OF A MAIN 
PROGRAM AND FIFTEEN SUBROUTINES AND ARE DESCRIBED BELOW. 


7 oh o Se of oo of ot oe ot ef Oe 


at 
oe 


THE FOLLOWING PARAMETERS ARE DEFINED: 
1, A-EFPFECTIVE CROSS-SECTIONAL AREA OF FLEXIBLE ARM 
2. ARRDD-3%3 SECOND TIME DERIVATIVE OF ROTOR RESIDUAL ACCELERATION 
MATRIX 
Seekun=245 ROTOR TRANSFORMATION HATRIX DIFFERENTIATED WITH RESPECT 
TO THETA 
Poe erPEOLIVE BULK MODULUS OF FLUID 
» BIGF-3X1 RIGHT-HAND SIDE VECTOR FOR LARGE AND SMALL MOTION 
ACCELERATIONS 
- BIGM-3X3 MATRIX OF LARGE AND SMALL MOTION ACCELERATION 
COEFFICIENTS 
7.CTM-TOTAL LEAKAGE COEFFICIENT OF THE ACTUATOR 
a 8. DEFM-DISPLACEMENT DEFORMATION VARIABLE 
9. DEFMD-TIME DERIVATIVE OF DISPLACEMENT DEFORMATION VARIABLE 
ae 10. DIFF ,QERR,QERR1,FACTOR-DUMMY VARIABLES 
BS revi 353 DEFORMATION MATRIX 
ae Peet l= ono) DEPORMATION@IAIRIX DIFFEREN@@ATED WITH RESPECT TO THE 
cg DISPLACEMENT DEFORMATION VARIABLE 
: 13. DL12-343 DEFORMATION MATRIX DIFFERENTIATED WITH RESPECT TO THE 


ON 


%& 3% 3c ct ck ck oof 3G cb ook ob 
in f 


a SLOPE DEFORMATION VARIABLE 

cy 14.DL1D-3X3 FIRST TIME DERIVATIVE OF DEFORMATION MATRIX 

ad 15. DM-ACTUATOR DISPLACEMENT 

ze ue. P -HOBURUse OF ELASTICITY OF SSTEEL 

% 17. FN-2X1 RIGHT-HAND SIDE VECTOR FOR SMALL MOTION ACCELERATIONS 
a 18. FQ-RIGHT-HAND SIDE FOR LARGE MOTION ACCELERATIONS 

a 19.G-3X1 GRAVITATIONAL ACCELERATION VECTOR 

7 Z0UmGrOs-o,1 GLOBAL POSITION VECTOR POR ARM TIP 

2“ Zend EAS LINK FIRST MOMENT OF INERTIA VECTOR 

a 22.H21-2X3 LINK SHAPE MATRIX FIRST MOMENT OF INERTIAVECTOR 
> 23.H41-1X3 LOAD FIRST MOMENT OF INERTIA VECTOR 

zy 24. KCE-TOTAL FLOW PRESSURE COEFFICIENT 

as 25. PL-LOAD HYDRAULIC PRESSURE DROP 

* 260) Sen OKAULTC SUPPLY PRESSURE 

* 27.QL-FLOW DELIVERED FROM THE SERVOVALVE 

* 28. SLOP-SLOPE DEFORMATION VARIABLE 


5] 


29. SLOPD-TIME DERIVATIVE OF SLOPE DEFORMATION VARIABLE 
30. SOL-3X1 VECTOR OF LARGE AND SMALL MOTION ACCELERATIONS 
31. TE-TORQUE EPPiciENnex 

32. TH-LARGE MOTION POSITION VARIABLE 

33. THD-TIME DERIVATIVE OF LARGE MOTION VARIABLE 

34. TORQUE-APPLIED TORQUE BY ACTUATOR 


** 35. U-2X1 ARM TIP DEFORMATION VECTOR INCLUDING DISPLACEMENT AND SLOPE 
* 36, UD-2X1 ARM TIP DEFORMATION VECTOR DIFFERENTIATED WITH RESPECT TO 
TIME 

* 37. VT-TOTAL COMPRESSED VOLUME INCLUDING ACTUATOR LINES AND CHAMBERS 
*  38,W-3X3 LINK TRANSFORMATION MATRIX 

* 39, WD-3X3 FIRST TIME DERIVATIVE OF LINK TRANSFORMATION MATRIX 

*  40.WRDD-3X3 SECOND TIME DERIVATIVE OF LINK RESIDUAL ACCELERATION 

x MATRIX 

* 41, WTH-3X3 TRANSFORMATION MATRIX DIFFERENTIATED WITH RESPECT TO 
THETA 

** 42, XIFRAC-VARIABLE FRACTIONAL AMOUNT OF INPUT CURRENT TO SERVO- 

ve VALVE 

** 43. XIINP-CURRENT INPUT EQUAL TO INITIAL AND FRACTIONAL AMOUNTS 

*  44.XIL-3X3 INERTIA MATRIX OF THE LOAD 

* 45. XIO-INITIAL INPUT CURRENT TO SERVOVALVE 

* 46, XIR-3X3 ROTOR INERTIA MATRIX 

*  47,XISTEP-STEP INPUT OF FRACTIONAL AMOUNT OF INPUT CURRENT 

* 48. XK11-2X2 PARTIAL LINK STIFFNESS MATRIX 

** 49, XKN-2X2 LINK STIFFNESS MATRIX 

** 50, XKV-SERVOVALVE SIZING CONSTANT 

“51, XLL-LENGTH OF FLEXIBLE ARM 

* 52. XNL-MASS OF LOAD 

* 53, XMNN-2X2 COEFFICIENT MATRIX OF SMALL MOTION ACCELERATIONS IN THE 
* SMALL MOTION DYNAMIC EQUATIONS 

* 54, XMNQ-2X1 COEFFICIENT VECTOR OF LARGE MOTION ACCELERATIONS IN THE 
* SMALL MOTION DYNAMIC EQUATIONS 

* 55. XMQN-1X2 COEFFICIENT VECTOR OF SMALL MOTION ACCELERATIONS IN THE 
* LARGE MOTION DYNAMICS EQUATION 

* 56, XMQQ-COEFFICIENT OF LARGE MOTION ACCELERATION IN THE LARGE MOTION 
vt DYNAMICS EQUATION 

* §7, XMQQP-2X2 DUMMY MATRIX FOR USE IN FORMULATING THE EQUATIONS OF 

v MOTION 

*  58.XMR-MASS OF ACTUATOR ROTOR 

* 59, XMU-MASS DENSITY OF STEEL FLEXIBLE ARM 

* 60. XMX-FIRST MOMENT OF LOAD WITH RESPECT TO THE LOCAL COORDINATE 

* Y AXIS 

* 61. XXI-VARIABLE REPRESENTING INERTIA-LIKE LOAD PROPERTY 

* 62, YYI-VARIABLE REPRESENTING INERTIA-LIKE LOAD PROPERTY 

* 63, ZI-AREA MOMENT OF INERTIA OF FLEXIBLE ARM 

cis 

* INITIAL VALUES OF PARAMETERS ARE INPUTTED VIA XINIT SUBROUTINE 
INITIAL 

D DIMENSION U(1 ),XMQQ(1),XMQQP(1 ),DL1(3,3),WTH(3,3) ,ARTH(3,3), 

D #XIR(3,3),XMQNC1 UDC 1 oa GG a ieee 

D WRDD( 3,3) ,DL1D(3,3) ,WD(3,3) ,ARRDD(3,3),H41(1,3),XK11(1_ ), 

D # XMNQ(1 ),W(3,3),XMNN(1 ),XKN(1 ),FN(1 ),BIGM(2,2), 

D #BIGF(2,1),XIL(3,3),DL11(3,3),DEFMD(1),SOL(2),THD(1), 

mg Ai), BC), Zl ,FQ(1),GPOS(3) ,XITH(1), 


a2 


D  #XMU(1),XLL(1),XML(1),XMR(1),XMX(1),TH(1),TORQUE( 1) ,DEFM(1), 
D  #PS(1), CTM(1),VT(1),BE(1),DM(1),XKV(1),TE(1), 

D #QL(1),PL(1),DIFF(1),XIINP(1),QERR1(1),QERR(1),FACTOR(1),XISTEP(1), 
ee? DEIG( 1), DETO(1),FIT(1),CTB(1),FCO(1)  ,DEPL(1), 

D  # DI(1),DEQ(1),FC1(1),UC(1) 

FIXED I 

NOSORT 

D COMMON/FCDATA/C1,C2, A1P,A2P,BETA1,BETA2 


ate 
rx 


C1=0. 515462194 
C2=-0. 205906794 
A1P=1. 362220557 
A2P=0. 981867539 
BETA1=1. 877920950 
BETA2=4. 701142847 
DELS=TAU 

EXCLUDE U,XMQQ,XMQQP,DL1,WTH, ARTH, 
XIR,XMON ,UD,H11,G,H21, 
WRDD,DL1D,WD,ARRDD,H41,XK11, ... 

XMNQ,W,XMNN,XKN, FN, BIGM, 

BIGF ,XIL,DL11.DEFMD,SOL,THD, 


Rais FQ,GPOS ,XITH, 
XMU, XLL, XML, XMR, XMX,TH, TORQUE , DEFM, 
PS, CTM,VT,BE,DM,XKV,TE, 


Gives v irr ,ATINP ,QERRI,QERR FACTOR, AISTIEP, «.. 
PEO. | Tl Cie Due , PREE Ee 1 Derr, DE Tel DEiGZ 
C2yect yee lAZ, CIl,AlE BETA] ,FO1 


* INITIALIZATION SUBROUTINE 
5 
CAliy SiNtT (TH, THD,DEFM,DEFMD wvO.FOo0 A.M. xMU Se. 
Nie Re. 2dr o SOU Vile DMA Rey -LE. Ol. ss 
PiteeolG DEG) 
a 
* WRITE( 6,1) XML 
7] FORMAT GIS. 5) 
DERIVATIVE 
COEFFICIENTS FOR BOTH LARGE AND SMALL MOTION ACCELERATIONS 
ve AND THE RIGHT-HAND SIDES ARE COMPUTED IN THE FOLLOWING 
* SUeROURI NES. AuLSO,THE HYDRAULIG DYNAMICS ARE INCLUDED 
* IN THE MAIN PROGRAM. 
NOSORT 
% HYDRAULIC DYNAMICS 
* KIS@TEPC 1 )=XIFRAC(1)*STEP(CO. 0) 
¥ PUNE Ginj=x1LOC))tXISTEP( 1 ) 
XIINP(1)=DEIC(1) 
TPeeLGipaGt.PS(1)) GO TO 2 
GO TOs 
Z PECL )=PS( 1) 
3 QERR1( 1)=(XIINP( 1)*XKV( 1)*DSQRT(FS(1)-PL(1)))-C(DM(1)*THD(1)) 


QERR(1)=QERR1(1)/CTM(1) 


Be 


DIFF(1)=QERR(1)-PL(1) 
FACTOR(1)=VT(1)/(4. ODO*BE( 1)*CTM(1)) 
DIFF1(1)=DIFF(1) 


SORT 
PLI=INTGRL( PLIC,DIFF1,1) 
NOSORT 
PLC 1)=PL1(1)/FACTOR( 1) 
TORQUE( 1)=TE(1)*PL(1)*DM(1) 
¥ TORQUE( 1)=DETO( 1) 
* TORQU=TORQUE( 1) 
* MATRIX AND VECTOR FORMULATION SUBROUTINE 
* 


CALL FORM(W,WTH,WD,DL1,DL1D,XIL,XIR,ARTH,WRDD,ARRDD,U,UD,... 
XMQQP ,G,H11,H21,DL11, H41,XK11,A,XMU,XML,XLL,TH,THD,... 
DEFM , DEFMD ,E,Z1,XMR,XMX ) 

TP1=TORQUE( 1) 


COEFFICIENT OF LARGE MOTION ACCELERATION IN LARGE MOTION DYNAMICS 
EQUATION SUBROUTINE 


CALL XLMMQQ(XMQQ,U,XMQQP,DL1,WTH, ARTH,XIL,XIR,A,XMU,TH,SP,... 
DEFM,MQQ1,TP) 
T6=TP 


COEFFICIENTS OF SMALL MOTION ACCELERATIONS IN LARGE MOTION DYNAMICS 
EQUATION SUBROUTINE 


CALL XLMMQN(XMQN,A,XMU,XML,XLL, XMX, DEFM ) 
RIGHT-HAND SIDE FOR LARGE MOTION DYNAMICS EQUATION SUBROUTINE 
CALL XLMFQ(FQ,U,XMQQP ,DL1,WTH,ARTH,XIL,XIR,UD,H11,G,H21,WRDD,... 
DL1D,WD,ARRDD,H41,TH,THD,DEFM,DEFMD, A,XMU,XML,XLL,... 
TORQUE ,FTT) 

LINK STIFFNESS MATRIX SUBROUTINE 


CALL SMKNCXKN, XK11, XMQQP,A,XMU, THD) 


COEFFICIENTS OF LARGE MOTION ACCELERATION IN SMALL MOTION 
DYNAMICS EQUATIONS SUBROUTINE 


CALL SMMNQ(XMNQ,DL1,WTH,XIL,DL11, W,TH, DEFM ,A,XMU,... 
Se 

MNQ=XMNQ( 1) 
RIGHT-HAND SIDE OF SMALL MOTION DYNAMICS EQUATIONS SUBROUTINE 


CALL SMFN(FN,H21,W,G,WRDD,DL1,XIL,DL11 ,WD,DL1ID,H41,TH,... 
THD , DEFM, DEFMD ) 


COEFFICIENTS OF SMALL MOTION ACCELERATIONS IN SMALL MOTION DYNAMICS 


54 


* EQUATIONS SUBROUTINE 


cE 


CALL SMMNN(XMNN,XMQQP, XML, A, XMU ) 
MNN=XMNN(1) 


ACCELERATION COEFFICIENTS MATRIX AND RIGHT-HAND SIDE VECTOR 
FORMULATION SUBROUTINE 


SE 3 se 


CALL BIGFOR(BIGM,BIGF,XMQQ,XMOQN,FQ,XMNQ,XMNN, XKN, FN,U, DEFMD) 


toh 


LINEAR EQUATION SOLVER FOR ACCELERATIONS SUBROUTINE 


bt 


CALL XLEQ(BIGM,BIGF,SOL) 


TRANSFORMATION FROM LOCAL COORDINATE TO GLOBAL COORDINATE TIP 
POSITION SUBROUTINE 


| ee ee ee a 


CALL GLOB(GPOS ,W, DEFM) 


of 


INTEGRATE ACCELERATIONS AND THEN VELOCITY TO GET LARGE MOTION 
* ANGULAR POSITION AND SMALL MOTION,LOCAL COORDINATE ,TIP POSITION 


DO 5 I=1,2 
SOL1(I)=SOL(I) 
5 CONTINUE 
SORT 
VEL=INTGRL( VO, SOL1,2) 
NOSORT 
THD(1)=VEL(1) 
DEFMD( 1)=VEL(2) 
* SLOPD(1)=VEL(3) 
DO 10 I=1,2 
VEL1(1)=VEL(I) 
10 CONTINUE 
SORT 
POS=INTGRL( POSO,VEL1,2) 
NOSORT 
TH(1)=PO0S(1) 
DEFM(1)=POS(2) 
AC1=SOL1(1) 
AC2=S0L1(2) 
VE1=VEL(1) 
VE2=VEL(2) 
PO1=P0S(1) 
PO2=POS(2) 
LA=PO01 
S=P02 


# CALL TRAIN(POS,XLL,THICK, STRANE) 
= STRAIN=STRANE( 1) 


TAG2=AC1+AC2/0. 9985D0 
TAG1=VE1+VE2/0. 9985D0 


a5 


TAG =PO1 +P02/0. 9985D0 


¥ 
ve 
Dededededelededede dete seds Sede eee Fee FeTTo EERE ELEC RE SEDER ETE LODE SETS STOVES TS TCT CTS TS TEE TE DETTE 
* CALL CCO(XMQQ, XMQN,XMNN ,XMNQ,FTT,FN,XKN,... 
* DEFM, CTB,FCO,FC1,DEFMD) 
T5=CTB( 1) 
ho 
SAMPLE 
ate 


DETO( 1)=RK+3. 9595*PO1-51. 2915*VE1 

* DETO( 1)=1. 66898*(-70. 0O*VE1+1225*( 1-PO1))+23. 7854*SIN(PO1) 
UC( 1)=RK-845. 7607*TAG-60. 0000*TAG1 
DETO(1)=UC(1) + 28.4392 

CALL DESCU(DETO,CTB,TAG1,TAG,FCO,GKV,GKP, DEA) 


*, ™ 
Peters Pe cedede Tete Fe Te Te ee eee ee eae TEES TOTS TOES TS TE TEES TS ES TOTNES TOTS TE TS TS AS TEES SEN ESTES 


* INVERSE HYDRAUIC DYNAMIC 
DEPL( 1)=CEDO( 1) (TEC) ) BM 1) 
DICIJ=VIC1)*(DEPL(1)= PL(1))/( 42000-2501) 0500cre, 
DEQ(1)=DM(1)*THD( 1) +CTM(1)*DEPL( 1) +DI(1) 
DEEP = Es( 1) eDEPLGiy 
FE CDEPP Se. Di Oe) pe iin 
DEIC(1)=4. ODO 


SOUOnsT> 
ENDIF 
DEIC1=DSQRT( DEFP) 
a GO TO 14 
os DEFP=-DEFP 
* DEIC1=DSQRT( DEFP) 


DEIC2=DEIC1*XKV( 1) 

DEIC(1)=DEQ( 1)/(DEIC2 ) 

TORK=DETO( 1) 

IF(DEIC(1) .GT. 4.0D0) DEIC(1)=4. ODO 
IF(DEIC(1) .LT. -4.0D0) DEIC(1)=-4. ODO 

15 CU=DEIC( 1) 
TERMINAL 
METHOD ADAMS 
CONTROL FINTIM=5.0000 ,DELT=0. 005 
PRINT 25. 0E-3, TAG, PO1,PO2,TORK,DEIC 
SAVE 25. 0E-3,TAG,PO1,P02,TORK,DEIC 
*ND 
*ARAM GKV=4.00 , GKP=4. 00 
*ND 
*ARAM GKV=8.00 , GKP=16. 00 
*RAPH (G1,DE=TEK618) TIME(UN=SEC) ,LA(LO=-0. 12,SC=0.15),... 
S( LO=-0. 12,SC=0. 15) 
*ABEL (G1) LOAD=0. 0000 KG 
*RAPH (G2,DE=TEK618) TIME(UN=SEC) , TAG, TAG] 
*ABEL (G2) LOAD=0.0000 KG 
*RAPH (G3,DE=TEK618)TIME , DEA 
*ABEL (G3) LOAD=0. 0000 KG 
*RAPH (G4, DE=TEK618)TIME(UN=SEC) , TAG( UN=RAD) 
*ABEL (G4) LOAD=0. 0C00 KG 
*RAPH (G5 ,DE=TEK618) TIME(UN=SEC) ,CU(UN=MA) , TORK( UN=N-M) 


*ABEL (G5) LOAD=0. 0000KG 
*RAPH (G1,DE=TEK618) TAG1,TS 
*ABEL (G1) LOAD=0. 0000 KG 
END 

STOP 


LISTING OF SUBROUTINES 
FORTRAN 


SUBROUTINE XINIT(TH,THD,DEFM,DEFMD ,VO,POSO,A,ML,MU,LL,MR, 
Hie Dl. PS. CIM, Vii wees Digi Tes OlarL,PLiG,DELC) 

REAL*8 VO(2),POSO(2),ML,MU,LL,MR,TH,THD,DEFM,DEFMD, 
#A,E,Z1,ITORQ,PS, CTM,VT,BE,DM,KV,TE,QL,PL,PLIC, 
CTE AP1, BETA DE le 
#DEPL , DEQ,DI,DEFP,DEIC1,DEIC2 

ITORQ= 23. 893030030000000 

DM=6. 2271D-05 

TE=. 90000000000000 

PL=ITORQ/(DM*TE) 

PLIC=PL 

CIM=3. 7064772D-13 

QL=CTM*PL 

KV=2. 402963D-09 

PS=1. 37888D+07 


= I0=QL/( KV*DSQRT(PS-PL) ) 

or IMAX=10. 000000000000000 

< IFRAC=. 4D0**( IMAX-I0) 
VT=3. 05127D-04 
BE=690. D6 


A=6.17795D-04 
ML=0. 0000000000000 
MU=7 861. 05000000000000 
LL=0. 99850000000000 

* STRANE( 1)=0. OOOO0000000000 
MR=9. 00011451000000 
E=2.0D11 
ZI=4. 065D-10 
V0( 1)=0. 000000000000000 
VO(2)=0. 000000000000000 
POSO0(1)=. 050931116 
POSO(2)=-. 0610213450000 
TH=PO0S0( 1) 
THD=VO( 1) 
DEFM=POS0O( 2) 
DEFMD=VO( 2) 


DEIC=0. 2 
* DETO= 40.5 
* PEPL=PLIC 
RETURN 
END 
ve 
ve 
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DOUBLE PRECISION FUNCTION ONE(X) 
REAL*8 C1,A1P,BETA1 
COMMON/FCDATA/C1,A1P, BETA1 


ONE= C1*( A1lP*(COS( BETA1*X )+COSH 
(BETA1*X) )+( SINC BETA1*X)+SINH( BETA1*X) ) ) 

RETURN 

END 


DOUBLE PRECISION FUNCTION TwO(X) 
REAL*8 C1,A1P,BETA1 
COMMON/FCDATA/C1,A1P, BETA 


TWO= (C1*( A1P*(COS( BETA1*X)+COSH 
( BETA1*X) )+(SIN( BETA1*X)+SINH( BETA1*X) ) ) ) 
WD 
RETURN 
END 


DOUBLE PRECISION FUNCTION SIX(X) 
REAL*8 C1,A1P,BETA1 
COMMON/FCDATA/C1,A1P, BETA] 


SIX= .9985D0**(C1*(A1P**(COS( BETA1*X)+COSH 
(BETA1*X) )+( SIN( BETA1**X )+SINH( BETA1*X) )))+ 
X*(C1*( A1P*(COS( BETA1*X)+COSH 
(BETA1*X) )+(SIN( BETA1*X )+SINH( BETA1*X) ))) 
RETURN 
END 


DOUBLE PRECISION FUNCTION EIGHT(X) 
REAL*8 C1,A1P,BETA1 
COMMON/FCDATA/C1,A1P,BETAI 


EIGHT= (C1*BETA1*BETA1*( A1P*( -COS 
( BETA1*X )+COSH( BETA1*X) )+( -SINC BETA1*X )+SINH( BETA1**X ) ) ) ) 
wwe D 
RETURN 
END 
DOUBLE PRECISION FUNCTION ONE(X) 
READ 6 C1. GZ. AIP,AZP, BEAL. BETAZ 
COMMON/FCDATA/C1,C2 »A1P,A2P,BETA] , BETA2 
ONE= C1*( A1P*( COS( BETA1*X )+COSH 


(BETA1*X) )+( SINC BETA1*X)+SINH( BETA1*X) ) )+ 
C2*( A2P*( COS( BETA2*X )+COSH( BETA2*X) )+ 


if (SINC BETA2*X )+SINH( BETA2*X) ) ) 


DOUBLE PRECISION FUNCTION TWO(X) 


ES 


FSeSLS .C1,C2, Ale, a2P, 8 
COMPION/ECDATA/C1,C2, Al 


TwO= (C1*(AlP=( COSC BETA1*xX )+COSH 


i: (BETA1*X) )+(SIN( BETA1*X )+SINH( BETA1*®X) ) )+ 

# C2*( A2P**( COS( BETA2**X )+COSH( BETA2**X) )+ 

is (SIN( BETA2*X)+S INH( BETA2*X) ) ) )**2 
RETURN 

END 

DOUBLE PRECISION FUNCTION SIX(X) 

REAL*8 €1,C2, AlP ,A2P, BETA, BETA2 

COMMON/FCDATA/C1,C2, A1P,A2P,BETA1,BETA2 


SIX= . 9985*(C1*(A1P*(COS( BETA1*X)+COSH 
¥ (BETAI* X) )+(SIN(BE TA1*X)+SINH(BETA1*X)))+ 
C2*(A2P*(COS( BETA2*X)+COSH( BETA2*X) )+ 

? (SIN( BETA2*X)+SINH( BETA2*X))))+ 
z: X*(C1*(A1lP*(COS( BETA1*X)+COSH 
di (BETAI*X) )+(SIN(BETA1*X)+SINH(BETA1*X) ) )+ 
i C2*(A2P*(CCS( BETA2*X )+COSH( BETA2*X) )= 
3 (SIN( BETA2*X)+SINH( EBETA2*¥)))) 


END 

DOUBLE PRECISION FUNCTION EIGHT(X) 

RBAES Ci, C2, Ber Sls, SA PRE TRZ 

COMMON /ECDAT4/C1,C2, AIP 422, BETA] ,BETA2 

EIcHT= (Cie Reie pe TAl* (ee (-COS 

? CBETA IS )+tCCSAC BET AL “> ) )+( - SENCBEPAL=X) HS INAGBET AI) ) + 
3 Gin eee ne 2* ( A2P * (COS GRE TAZ ay COSHH BETA s))+ 
2 (-SINCBETE2=* ) +S IMH(BETA2“E))))**2 
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SUBROUTINE FORM(W,WPE,WD,DL1,DLID,XIL,XIR,ARTE,WRDD ,ARRDD,U,UD, 
#xMQQP ,G,H11,H2i,DLil H&1 SKIL, AyMyML, LL, TH “THD, DEPN,DEEMD , 
# E,Z2,MR,MX ) 

PEAU*8 (3,3) ,WEH(3,3),WD(3,3),DL1(3,3),DL1D(3,3),XIL(3,3) 

REALS XIR(3,3),4RTH(3,3) WRDD(3,3),422DD(3,3),U ,UD 

REAL*8 XMQOP ecs.1), Weel, 3), 92161.3),,90001(3 53) 

REAL8 H41(1,3),XKil JMU, ML,LL,MR,MX, TE 

REAL*8 THD,DEFM,DEFMD ys Be Zi 

REAL*8 C1,ALP,BETAI 

REAL*8 ONE, TWO,E5IGHT,11,14,16 


ETERNAL ONE , TWO, EIGHT 

MON /FCDATA/C1,41P,BETAI 

oes 00000000900000 

W(1,2)=0. 00000000020000 
W(1,3)=0. 00000000060000 
W(2, 1)=LL*DCOS( TE 

We2 2)=DC0S( Tr 


oC me: 


W(2,3)=-DSIN( TH 


By 


-— se 


Se 


W(3,1)=LL*DSIN(TH) 
W(3,2)=DSIN(TH) 
W(3,3)=DCOS(TH) 

WTH( 1, 1)=0. 00000000000000 
WTH(1,2)=0. 00000000000000 
WTH( 1, 3)=0. 00000000000000 
WTH(2,1)=-LL*DSIN(TH) 
WTH(2,2)=-DSIN(TH) 
WTH(2,3)=-DCOS(TH) 

WTH( 3, 1)=LL*DCOS(TH) 
WTH(3,2)=DCOS(TH) 
WTH(3,3)=-DSIN(TH) 
WD(1,1)=0. 00000000000000 
WD(1,2)=0. 00000000000000 
WD(1,3)=0. 00000000000000 
WD(2,1)=-LL*DSIN(TH)**THD 
WD(2,2)=-DSIN(TH)*THD 
WD(2,3)=-DCOS(TH)*THD 
WD(3,1)=LL*DCOS(TH)*THD 
WD(3,2)=DCOS(TH)*THD 
WD(3,3)=-DSIN(TH)*THD 
DL1(1,1)=1. 00000000000000 
DL1(1,2)=0. 00000000000000 
DL1(1,3)=0. 00000000000000 
DL1(2,1)=0. 00000000000000 
DL1(2,2)=1. 00000000000000 
DL1(2,3)=-1. 378573350D0*DEFM 
DL1(2,3)=-0. 00000**DEFM 
DL1(3,1)=DEFM 

DL1(3,2)= 1. 378573350D0*DEFM 
DL1(3,2)=0. 00000*DEFM 
DL1(3,3)=1. 00000000000000 
DL1D(1,1)=0. 00000000000000 
DL1D(1,2)=0. 00000000000000 
DL1D(1,3)=0. 00000000000000 
DL1D(2,1)=0. 00000000000000 
DL1D(2,2)=0. 00000 
DL1D(2,3)=-1. 378573350D0**DEFMD 
DL1D(2,3)=-0. 0000000*DEFMD 
DL1D(3,1)=DEFMD 

DL1D(3,2)= 1. 378573350D0*DEFMD 
DL1D(3,2)=0. 0000000**DEFMD 
DL1D(3,3)=0. 00000000000000 
XIL(1,1)=ML 

XIL(1,2)=0. 00000000000000 
XIL(1,3)=. 00000000000000 
XIL(2,1)=0. 00000000000000 
XIL(2,2)=0. 0000000000 
XIL(2,3)=0. 00000000000000 
XIL(3,1)=. 00000000000000 
XIL(3,2)=0. 00000000000000 
XIL(3,3)=0. 0000000000 
MX=0. 00000000000000 

XXI=0. ODO 

YYI=0. ODO 


XIR(1,1)=MR 

XIR(1,2)=0. 000000000000000 
XIR(1,3)=0. 000000000000000 
XIR(2,1)=0. 000000000000000 
XIR(2,2)=. 027467130000000 
XIR(2,3)=0. 000000000000000 
XIR(3,1)=0. 000000000000000 
XIR(3,2)=0. 000000000000000 
XIR(3,3)=. 02746713000000 
ARTH(1, 1)=0. 00000000000000 
ARTH(1,2)=0. 00000000000000 
ARTH(1,3)=0. 00000000000000 
ARTH(2,1)=0. 00000000000000 
ARTH(2,2)=-DSIN(TH) 
ARTH(2,3)=-DCOS(TH) 

ARTH( 3, 1)=0. 00000000000000 
ARTH( 3, 2)=DCOS(TH) 
ARTH(3,3)=-DSIN(TH) 

WRDD( 1, 1)=0. 00000000000000 
WRDD(1,2)=0. 00000000000000 
WRDD(1,3)=0. 00000000000000 
WRDD(2, 1)=-LL*DCOS(TH)**( THD***2) 
WRDD( 2, 2)=-DCOS(TH)**( THD**"2 ) 
WRDD(2,3)=DSIN(TH)*( THD***2) 
WRDD(3, 1)=-LL*DSIN(TH)*( THD**2) 
WRDD(3,2)=-DSIN(TH)*(THD****2) 
WRDD( 3, 3)=-DCOS(TH)**( THD***2) 
ARRDD(1,1)=0. 00000000000000 
ARRDD(1,2)=0. 00000000000000 
ARRDD(1,3)=0. 00000000000000 
ARRDD(2,1)=0. 00000000000000 
ARRDD( 2, 2)=-DCOS(TH)**( THD*"*2) 
ARRDD(2,3)=DSIN(TH)**( THD***2) 
ARRDD( 3, 1)=0. 00000000000000 
ARRDD( 3, 2)=-DSIN(TH)**( THD****2 ) 
ARRDD( 3, 3)=-DCOS(TH)**( THD***2) 
U =DEFM 


UD =DEFMD 
CALL DQG4(-LL, 0. DO,TWO,1I1) 
XMQQP =T1 


XMOQP =0. 3714286 
G(1,1)=0. 00000000000000 
G(2,1)=0. 00000000000000 
G(3,1)=-9. 80660000000000 
H11(1,1)=4. 85651900000000 
H11(1,2)=-2. 42825869300000 
H11(1,3)=0. 00000000000000 
H21(1,1)=0. 00000000000000 
H21(1,2)=0. 00000000000000 
CALL DQG4(-LL,-0.DO0,ONE ,I4) 
H21(1,3)=A*MU*I4 
H21(1,3)=A*MU*(0. 50000) 

DO 50 I=1,3 

DOmeonJ=1.3 

DL11(1I,J)=0. 00000000000000 
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CONTINUE 
CONTINUE 

DL11(3,1)=1. 00000000000000 
H41(1,1)=ML 

H41(1,2)=0. 000000000000000 
H41(1,3)=. 000000000000000000 
CALL DQG4(-LL, 0. DO, EIGHT, I6) 
xin =I6*E*ZI 

RETURN 

END 
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SUBROUTINE XLMMQQ( XMQQ,U, XMQQP,DL1,WTH, ARTH, XIL,XIR,A,MU,TH,SP,DEF 


1M,MQQ1,TP) 


REAL**8 XMQQ,UT ,P ,DLIT(3,3),WIHT( 3, 3)sARTHTC 3.3) ee ues 
REAL*8 P2(3,3),P3(3,3),P4(3,3),P5(3,3),P6(3,3),P7(3,3),MU 

REAL*8 U ,XMQQP ,DL1(3,3),WIH(3, 3) ,ARTHOS 3) RIC oan 
REAL*8 XIR(3,3),4,TH, DEFM, SP, TP,MQQ1,MQQ2 


M=2 
L=1 
K=3 


SNQQ=0. COOODD000000000 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


MQQ1= 
MQQ2= 


TRANS(U,UT,L,L) 
MATMUL(UT,XMQQP,L,L,L,P) 
MATMUL(P,U,L,L,L,SP) 
TRANS(DL1,DL1T,N,N) 
TRANS( ARTH, ARTHT ,N,N) 
TRANS(WTH, WIHT,N,N) 
MATMUL(WTH, DL1,N,N,N,P1) 
MATMUL(P1,XIL,N,N,N, P2) 
MATMUL( P2,DL1T,N,N,N,P3) 
MATMUL( P3 , WIHT,N,N,N,P4) 
MATMUL( ARTH, XIR,N,N,N,PS5) 
MATMUL( P5 , ARTHT,N,N,N,P6) 
MATADD( P/: ,P6,N,N,P7) 
TRACE( P7,N,TP) 

(1. D0/3. DO)*A*MU 

A*MU*SP 


ANOG=sOOl-MOC2 4 a Le 


WRIDEGs 52) Se 


RETURN 


END 


SUBROUTINE XLMMQN(XMQN,A,MU,ML,LL,MX , DEFM ) 
REAL*8 XMQN MU, ML,LL,MX,A, DEFM 
REAL*8 C1,A1P,BETA1,SIX,I9 ,C2,A2P,BETA2 
EXTERNAL SIX 
COMMON/FCDATA/C1,C2,A1P,A2P ,BETA1 
oY Vii vole CounEpMOMIFOINcHD.@ Jac 

XMOQN =(ML*LL)+MX+(A*MU* 19) 


, BETA2 


Be 


RETURN 
END 
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SUBROUTINE XLMFQ(FOQ,U,XNQQP,DL1,WIH,ARTH,XIL,XIR,UD,H11,G¢,H21, 


40 
50 


REAL*8 
one oP 1, DPiy GPP i GHP) , FTI 


REAL 
i=2 
L=1 
N=3 
CALL 
ROP 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


#WRDD ,DL1D,WD,ARRDD, H41 , TH, THD DEFN, DEFMD, A MU, ML, LL, 

# TORQUE, FIT) 

REAL*8 FQP 38 wen) ORE CC leo eb), 3). P4033) oS; 3) 
are emo 3 ),P705,50h0(3,5),29(3,3),P1003,3),P11(1,3),P12(1,3) 
REAL*8 FPFH(3,3),FHP(3,3),FPT(3,3),DL1DT(3,3) ,WDT(3,3),ARRDDT(3,3) 
REAL*8 UT SDUIM(S.3 ), WRDDICs,3),FPFC3,3),FPS(3,3),WEHT(3,3) 
REAL*8 U , XMQQP sols) 5) ih iteeme) ,ARTHCO 3), REEGS 3) 
REAL*8 XIR(3,3),UD ole Cl 3) GCs.) 2101.3), WRODC33S) 

Rea SeVbl0(3,3),ND( 3,3) ,ARRDD( 3,3) ,H41(1,3),NU,LL,ML 


PORQUE tO, PH, tiiy DEFM,DEPMD 


TRANS(U,UT,L,L) 
=XMQQP *THD 
MATMUL( UT, FQP,L,L,L,P) 
NATMUL(P,UD,L,L,L,FP1 
TRANS(WTH , WIHT N,N) 
MATMUL(H11 ,WIHT ,L,N,N 
MATNUL(P1,G,L,N,L,SP1 
MATMUL( UT ,H21,L, Ns P 
MATMUL( P2 ,WTHT,L,N,N 
MATMUL( P3,G,L,N,L,TP1 
TRANS(DL1,DL1T,N,N) 
TRANS(WRDD , WRDDT N,N) 
MATMUL(WTH, DL1,N,N,N, P4) 
MATMUL( P4 ,XIL,N,N,N, P5) 
MATMUL( P5 ,DLIT,N,N,N, P6) 
MATMUL( P6 ,WRDDT,N,N,N, FPF) 
TRANS(DL1D,DL1DT,N,N) 
TRANS(WD,WDT,N,N) 
MATMUL(WTH,DL1,N,N,N,P7) 
MATMUL( P7 ,XIL,N,N,N, P8) 
MATMUL( P8 ,DLIDT,N,N,N,P9) 
MATNUL(P9 ,WDT,N,N,N, FPS) 
TRANS( ARRDD, ARRDDT,N,N) 
MATMUL( ARTH, XIR,N,N,N,P10) 
MATMUL( P10, ARRDDT,N,N,N,FPT) 


En) 


P1) 
2) 
ES) 


DO 30 1=1,3 

DO 40 J=1,3 
Beier =P PS(l,J)*2. 
CONTINUE 


CONTINUE 

Cima TADDC FPF ,FPS,N,N,FPFH) 
Geen tADD( FPFH,FPT,N,N,FHP) 
Grogs TRACECFHP,N, TFPI) 

CALL MATMUL(H41,DLIT,L,N,N,P11) 
Sor ieieiUL( Pi WIHT,L,N,N,P12) 


oe) 


CALE MATMULC P1256 ji, eee) 
FIT=(*2. sA*MU FR ese (Pd TEP ie 
1s ES 2G Meare 1 Jolt I 

FQ=FTT+TORQUE 

RETURN 

END 
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SUBROUTINE SMKN( XKN, XK11,XMQQP,A,MU, THD) 


REAL**8 XKN ,KNP ,XMQQP oxen ,A, THD, MU 
KNP =XMQQP *( -A)*MU*( THD**2) 
XKN =KNP +XK11 
RETURN 
END 
SUBROUTINE SMMNQ(XMNQ,DL1,WTH,XIL,DL11, W,TH, DEFM A, MU, 
4#4LL) 
REAL*8 XMNQ .DL11T(3, 3), WiC 3,3) be. 3) eo ene) 
REAL*8 P3(3,3),P4(3, 3) ,DL1(3,3) ,WIH(3,3) ,XIL(3,3) 
READ Gms aonb tye TH, DEFM ,A,MU, LL 
REAL*8 C1,A1P,BETA1,TFP1 
REA e SIX ela ,C2,A2P, BETA2 


EXTERNAL SIX 
COMMON/FCDATA/C1,C2, A1P,A2P,BETA1 ,BETA2 
M=2 

Le 

N=3 

CALL TRANS(DL11,DL11T,N,N) 

CALL TRANS(W,WT,N,N) 

CALL MATMUL(WTH,DL1,N,N,N,P1) 

CALL MATMUL(P1,XIL,N,N,N,P2) 

CALL MATMUL(P2,DL11T,N,N,N,P3) 

CALL MATNUL(P3 ,WT,N,N,N,P4) 

CALL TRACE(P4,N,TFP1) 


WRITE(* ,*)LL 


GAGE DOGG CEL; O70 0he elas) 

XMNQ =TFP1 + (I11*A*MU) -( A*MU"0505 ise 
XMNQ=TFP1+( A*MU*I11) 

WRB s )a 


RETURN 
END 


SUBROUTINE SMFNCFN,H21,W,G,WRDD,DL1,XIL,DL11 »WD,DL1D,H41,TH, 


THD , DEFM, DEFMD 


REAL*8 FN ,P1(1,3) ,P2(3,3),P3(3,3),P4(3,3) ,P5(3,3),P6(3,3) 
REAL*8 P7(3,3),P8(3,3),P9(3,3) 
REAL*8 P14(1,3),P15(1,3) ,TP FP SP 
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REAL*8 FN1(3,3) Noone SL WRDDCS 3), DLIDCS, 3) 
Rio WHC Sys )na it 3), sbCs.3)W(3,3),DL11(3;3) 


hee) DEC 3.55 ),DLI1T( 3,3) We 3 5 3)) 
Peeees th, PHO, DERM DEPMD ea il INS 
a2 

L=1 

N=3 


CALL TRANS(W,WT,N,N) 
CALL MATMUL(H21,WT,L,N,N,P1) 
CALL MATMUL(P1,G,L,N,L,FP) 
CALL TRANS(DL11,DL11T,N,N) 
CALL MATMUL(WRDD,DL1,N,N,N,P2) 
CALL MATMUL( P2,XIL,N,N,N,P3) 
CALL MATMUL(P3,DL11T,N,N,N,P4) 
CALL MATMUL(P4,WT,N,N,N,P5) 
CALL MATMUL(WD,DL1D,N,N,N,P6) 
CALL MATMUL( P6,XIL,N,N,N,P7) 
CALL MATMUL(P7 ,DL11T,N,N,N,P8) 
CALL MATMUL(P8 ,WT,N,N,N,P9) 
DO 10 I=1,3 
ome =1 3 
P9(I,J)= P9(I,J)*2. 

20 CONTINUE 

10 CONTINUE 
CALL MATADD(P5,P9,N,N,FN1) 
CALL TRACE(FN1,N,TFN1) 
SP =TFN1 
CALL MATMUL(H41,DL11T,L,N,N,P14) 
CALL MATMUL(P14,WT,L,N,N,P15) 
CALL MATMUL(P15,G,L,N,L,FN3) 


TP =FN3 
FN =FP - SP ae 
RETURN 
END 
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SUBROUTINE SMMNN(XMNN,XMQQP,ML,A,MU ) 
REAL*8 XMNN ,XMQQP ,ML,MU,A 

* DO 10 I=1,2 

ze DO 20 J=1,2 

* XMNN( I,J)=0. 00000000000000 


* 20 CONTINUE 
*10 CONT INUE 


XMNN =ML 
* XMNN( 1, 2)=MX 
* XMNN( 2 ,1)=MX 
7% XMNN(2,2)=XXI+YYI 
oe BO 307 1=1, 2 
* DO 40 J=1,2 
AMNN =XMNN + XMQQP *A*MU 


* 40 CONTINUE 
* 30 CONTINUE 


as WRITE(C* ,*)XMQQP 


t 


° ° e ° 


* e e e 


ue 


20 
10 


RETURN 
END 
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SUBROUTINE MATMUL(A,B,M,L,N,C) 
REAL*8 A(M,L),BCL,N),C(M,N) 

DO 10 I=1,M 

DO 20 J=1,N 

C(I,J)=0. 0 

DO 30 INDEX=1,L 

C(I,J)=C(1,J) + ACI, INDEX)*B( INDEX, J) 
CONTINUE 

CONTINUE 

CONTINUE 

RETURN 

END 
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SUBROUTINE TRANS(A,B,M,L) 
REAL*8 AC(M,L),BCL,M) 

DO 10 I=1,M 

DO 20 J=1,L 
B(CJ,I)=ACI,J) 

CONTINUE 

CONTINUE 

RETURN 

END 


eeeeeeeeeee%e*#¢#?e#?8# #28 @ @ 8@ © @ @ © © @ @©@ © &@ © © © & & © © © © © © © © © & © © & © & © © © & © & © © © € €& & &€ © & © © 8 & F&F 8 


SUBROUTINE TRACE(A,M,TRAC) 
REAL*8 A(M,M) , TRAC 
TRAC=0. 0 

DO 10 I=1,M 

TRAC=TRAC + A(I,I) 
CONTINUE 

RETURN 

END 


MATRIX ADDITION SUBROUTINE 


SUBROUTINE MATADD(A,B,M,L,C) 
REAL*8 A(M,L),B(M,L),C(M,L) 
DO 10 I=1,M 

DO 20 J=1,L 

C(I,J)=AC(I,J) + BCI,J) 
CONTINUE 

CONTINUE 

RETURN 

END 
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SUBROUTINE BIGFOR(BIGM, BIGF,MQQ,XMQN,FQ,XMNQ,XMNN,XKN,FN,U, 


4#DEFMD) 
REAL*8 BIGM(2,2),BIGF(2,1),XMOQN , XMNQ , XMNN , XKN 
REAL*8 FN ,U ,MQQ,P ,FQ ,DEFMD 

M=2 

L=1 


BIGM(1,1)=MQQ 
BIGM(1,2)=XMON 
BIGM(1,3)=XMON(1,2) 
BIGM(2,1)=XMNOQ 
BIGM(2,2)=XNNN 
BIGM(2,3)=XMNN(1,2) 
BIGM(3,1)=XMNQ(2,1) 
BIGM(3,2)=XMNN(2,1) 
BIGM(3,3)=XMNN(2,2) 
BIGF(1,1)=FQ 

CALL MATMUL(XKN,U,L,L,L,P) 
BIGF(2,1)=FN -P +14, 4**DEFMD 
RETURN 

END 


be 


SUBROUTINE XLEQ( BIGM,BIGF,SOL) 
REAL*8 BIGM(2,2),BIGF(2,1),SOL(2) ,WKAREA( 18) 
M=1 
= 
CALL LEQT2F (BIGM,M,N,N,BIGF,M,WKAREA, IER) 
DO 10 I=1,2 
SOL(1)=BIGF(I,1) 
10 CONTINUE 


RETURN 
END 

SUBROUTINE GLOB(GPOS ,W,DEFM) 
REAL*8 GPOS(3),W(3,3),DEFM,RL(3) 
RL(1)=1. ODO 

RL(2)=0. ODO 

RL(3)=DEFM 

N=3 

=i. 

CALL MATMUL(W,RL,N,N,L,GPOS) 
RETURN 

END 


SUBROUTINE CCO(XMQQ,XMON,XMNN,XMNOQ,FTT,FN,XKN,DEFM,CTB,FCO,FC1, 
#DEFMD) 
REAL**8 XMQQ,XMNN,XMON,FTT,FN,XKN,U,CTB,FCO,CTA,B11,B22 
REAL*8 B33,B44,F11,F22,DEFMD,F33 
B11=XMQQ/0. 9985 
B22=XMOQN-B11 


¢7) 
“~ 


B33=XMNQ/0. 9985 
B44=XMNN-B33 
CTA=B22/B44 


CTB=XMQQ-CTA*XMNQ 


F1i1=CTA*FN 
F22=( CIA*XKN) DEE 
F33=(CTA* 14.4*DEFMD ) 
FCO=F ti-F22-F Tl =r oe 
PCier Wier ri 


RETURN 
END 
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SUBROUTINE LEQT2F (IMSL) 


PURPOSE 


USAGE 


ARGUMENTS 


A - 


I DKE IE 


WKAREA - 


IER = 


LINEAR EQUATION SOLUTION - FULL STORAGE 
MODE - HIGH ACCURACY SOLUTION 


CALL LEQT2F (A,M,N,IA,B, IDGT,WKAREA, IER) 


INPUT MATRIX OF DIMENSION N BY N CONTAINING 
THE COEFFICIENT NATRKIA OF THE EQUATION 
AX = B. 

NUMBER OF RIGHT-HAND SIDES. (INPUT) 

ORDER OF A AND NUMBER OF ROWS IN B. CINPUT) 
ROW DIMENSION OF A AND B EXACTLY AS SPECIFIED 
IN THE DIMENSION STATEMENT IN THE CALLING 

PROGRAM. (INPUT) 

INPUT MATRIX OF DIMENSION N BY M CONTAINING 
THE RIGHT-HAND SIDES OF THE EQUATION AX = B. 
ON OUTPUT, THE N BY M MATRIX OF SOLUTIONS 
REPLACES B. 

INEUT “OPTION: 

IF IDGT IS GREATER THAN 0, THE ELEMENTS OF 
A AND B ARE ASSUMED TO BE CORRECT TO IDGT 
DECIMAL DIGITS AND THE ROUTINE PERFORMS 
AN ACCURACY TEST. 

IF IDGT EQUALS 0, THE ACCURACY TEST IS 
BYPASSED. 

ON OUTPUT, IDGT CONTAINS THE APPROXIMATE 
NUMBER OF DIGITS IN THE ANSWER WHICH 
WERE UNCHANGED AFTER IMPROVEMENT. 

WORK AREA OF DIMENSION GREATER THAN OR EQUAL 
TO! N*=*243N, 

ERROR PARAMETER. (OUTPUT) 

WARNING ERROR 
IER = 34 INDICATES THAT THE ACCURACY TEST 

FAILED. THE COMPUTED SOLUTION MAY BE IN 
ERROR BY MORE THAN CAN BE ACCOUNTED FOR 
BY THE UNCERTAINTY OF THE DATA. THIS 
WARNING CAN BE PRODUCED ONLY IF IDGT IS 
GREATER THAN O ON INPUT. (SEE THE 

CHAPTER L PRELUDE FOR FURTHER DISCUSSION. ) 

TERMINAL ERROR 
IER = 129 INDICATES THAT THE MATRIX IS 


c8 


ALGORITHMICALLY SINGULAR. (SEE THE 
CHAPTER L PRELUDE). 
IER = 131 INDICATES THAT THE MATRIX IS TOO 
ILL-CONDITIONED FOR ITERATIVE IMPROVEMENT 
TO BE EFFECTIVE. 
REQD. IMSL ROUTINES - SINGLE/LUDATN, LUELMN , LUREFN , UERTST ,UGETIO 
- DOUBLE/LUDATN, LUELMN, LUREFN,UERTST, UGETIO, 
VXADD , VXMUL, VXSTO 


SUBROUTINE LEQT2F (A,M,N,IA,B,IDGT,WKAREA, IER) 


DIMENSION AC IA,1),BC1IA,1),WKAREA( 1) 

DOUBLE PRECISION A,B,WKAREA,D1,D2,WA 
FIRST EXECUTABLE STATEMENT 
INGTTARIZE LER 


DO 5 I=1,N 
WKAREA( JJ)=A(1,L) 
JJ=IJ+1 
5 CONTINUE 
DECOMPOSE A 
CALL LUDATN (WKAREA,N,N,A,IA,IDGT,D1,D2,WKAREA(J) , WKAREA(K), 
X* WA, IER) 
MICIERNGEs 126) GO! TO 25 
ie; UDG eon Or. OR. IER NE: 0) KK = 1 
pO 15 I=1,M 
PERFORMS THE ELIMINATION PART OF 
AX = B 
CALL LUELMN (A,IA,N,B(1,1),WKAREA(J) , WKAREA( MM) ) 
REFINEMENT OF SOLUTION TO AX = B 
IF (KK .NE. 0) 
** CALL LUREFN (WKAREA,N,N,A,IA,B(1,1),IDGT,WKAREA(J) ,WKAREA( MM), 
* WKAREA(K) , WKAREA(K) , JER) 
DO 10 II=1,N 
B(II,1) = WKAREA(MM1+I1) 
10 CONTINUE 
IF (JER.NE.0) GO TO 20 
15 CONTINUE 
GO TO 25 
20 TER = 13% 
25 JJ=1 
DO 30 J = 1,N 
DO 30 I =1,N 
A(1,J)=WKAREA( JJ) 
JJ=JI+1 
30 CONTINUE 
IF (IER .EQ. 0) GO TO 9005 
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9000 CONTINUE 
CALL UERTST (IER, 6HLEQT2F) 
9005 RETURN 
END 


at 
a) 


,  ,  , ] 


SUBROUTINE DQG4 (IMSL SUBROUTINE) 


PURPOSE 
TO COMPUTE INTEGRAL(FCT(X), SUMMED OVER X FROM XL TO XU) 


USAGE 
CALL DQG4 (XL,XU,FCT,Y) 
PARAMETER FCT REQUIRES AN EXTERNAL STATEMENT 
DESCRIPTION OF PARAMETERS 
AL - DOUBLE PRECISION LOWER BOUND OF THE INTERVAL. 
XU - DOUBLE PRECISION UPPER BOUND! OF Sane yen 
FCT - THE NAME OF AN EXTERNAL DOUBLE PRECISION FUNCTION 
SUBPROGRAM USED. 
Y - THE RESULTING DOUBLE PRECISION INTEGRAL VALUE. 
lS TN Sk One, 
: EVALUATION IS DONE BY MEANS OF 4-POINT GAUSS QUADRATURE 
oe FORMULA, WHICH INTEGRATES POLYNOMIALS UP TO DEGREE 7 
a EXACTLY. FORGRERE RENCE ore 
7% V. I. KRYLOV, APPROXIMATE CALCULATION OF INTEGRALS, 
* MACMILLAN, NEW YORK/LONDON, 1962, PP. 100-111 AND 337-340: 


oa SA A 


af. 
“. 


SUBROUTINE DQG4(XL,XU, FCT,Y) 
DOUBLE PRECISION XL,XU,Y,A,B,C,FCT 
WRITE(* = )eL, XU, ¥ 


A=. 5D0**(XU+XL) 
B=XU-XL 

C=. 43056815579702629D0*B 

Y=. 17392742256872693D0""( FCT( A+C )+FCT(A-C)) 

C=. 16999052179242813D0*B 

Y=B:"( Y+. 32607257743127307D0*(FCT(A+C)+FCT(A-C))) 


ner URN 
END 
* 
7 SUBROUTINE TKAIN(POS,LL,THICK,STRANE) 
* REAL*8 POS(2), LL, THUGK()) (sik ANE Gin 
: REAL*8 C1,A1P,BETA1 
chy COMMON/FCDATA/Ci,A1P,BETA1 
* =-LL/2. DO 
a STRANE(1)=THICK(1)*((POS(2) *((C1*BETA1*BETA1*( A1P*( -COS 


“ # (BETA1*X)+COSH( BETA1*X) )+( -SIN( BETA1*X )+SINH( BETA1*X) )))))) 
¥ RETURN 
veoe END 
rh) a har hae badge oe eh Bria peak hee ashi ter aria tak keer vicky dri} 
SUBROUTINE DESCU(CDETO,CTB ,TAG1,TAG,FCO,GKV,GKP, DEA) 
REAL*8 DETO1,DETO2,DETO,GKV,GKP,DEA, TAG1,TAG, FCO,CTB 
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DETO1=( -GKV*TAG1 )+GKP*( DEA-TAG ) 
DETO2=CTB*DETO1] 

DETO=DETO2+FCO 

RETURN 

END 


APPENDIX B 


POINT TO POINT CONTROL PROGRAM 
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LY this program 16 written te 
to tar the single-link 


estabnl 
TlLexible arm. 


PNUD es Ali vicr Ser OF” 
SNIPE. AteenkeS. FUR’ 


ish 


ied fae Gat 


Bea ie iby inl 


MEAL#OVL, N,FiEY,FHID,L,FS,VI, KE, UM, IM, RFHIDUI, 


/ PD,V3,VALIO01), 


Copier ee tio ei hw lorie bled ol, 


/\ACIOO1I,FACIOO1I,PDI, 


/ERPS,DEHI,51,V1,bu,FPHIDDI, Vt, PHL, THe, FDUDUI CiGu1), 


/PDUUT, TUR CLO00), T,RPLSUM 
REALH4 RAE, DI,De, ke, EV, HE, 


INtRISER eH ADISALNS (C16), ADLHANC 1G), LILUN lint lel, 
ALDEVID, DEVE LIS, SUAN, DAVAL, DAVALZ,Y,SIAIUS, 


JPHIDD, DALCLOQO), HASEADE 
LUMMUN/ICUNE Lin7 LCONF Lis 
EWU IVALENLE 
(ee UN Pi Otel DEV IDS DEV ie 
eeu Liat DEVE IAlsa) DE VPiiags. 


PISUNF Lis CR RASE ADE), HASEADE) , 


7 CLUCONE Dis CRLSUAND, SIAN), CLICUNE JI CRICICHANNELS 3, HAND 


I [he variables are detined 
l. except as noted below. 
RPATE=S0, OU0O 
L=O. SIR SyUVUUOU 
Po). o/6eoavpe, 
Vi=S. Ost 27 p-Ue 
Be =6'20., ODUG 
bim=o. ee / 1 D-OS 
fied. (Veo 7 2b Ss 
Ju J 
Err =U, UL) 
Lo FOULIG iS a Gummy variable 
rig | ast. wl 
Pee gla Coe) 
1S a dtimmy variabpie 
PRO, ODO 
HESS thy J at bth 
Ee. J/60/ 
EV=60. O00W) 
Lo FDSUM 15 the value 
FDSUM=0, Q0D0 
Pie) 0610-1 G4 oDO 
Y=VOdy 
165 the initial pressure load 
PLi=2s.dIS03U0/ (DM*EF FD 
Plew=e., OUDOXDEIANCI. OD) 


in 


ss 2 Cirle 


Cy tHb used tire 


mt tne filtere 


red 


the 


TIMP U e 


gd small 


drip 


Shirgiies ct 


larqe 


anqle 


the simulation pragram FIG 


(1 dl ebiske 
angie vebority. 
Slqnal. 


UPENCUNII=15,F 1LE=’ PHEB EOF. DAT’, SIALUS=’ NEW’ D 


I. NLP LALIT ZE THE BUARD 
SIAIUS=ALINII ©) 
StALUS=AL SHC) 
STAVUS=ALSF CRAIE) 


STATUS =ALISL. C LIUNE Lia) 
STATUDS=ALDV CU, V2 


as 


Sao 

IDEMI LEY THE KUN TRUL FARAPE TERS 
Well i *,*) 7LNFUL TRANSFER FUNLILUN CHRD? 
HEAL, HE, 
WekiEG*, #2 TAUNFUI GALNS CER, RV) (REALD? 
READE, EE, KY 


* wk KOK 


WRiTh(#,%*#) 7INRFUT THE DESIEED AkKM FUS1I1I1ON 


RE AD#, FHIDD1 
Ee EN it. Pee? 
PHID=C€ (2e#F [6/180) *FHIDD1») 
PRINT #, FHI D 
= LUMMENCE FUSTIIONINa UF THE ARM 
S1l=10.,O0OD0/2. OF8D05 
RIE =HE. FH LD 
U=QOD0 
Ji=1 
- Heqin data acquisitian pracess 
Oa, STATUS=ALAVEL, 1, DAVAL) 
SIAIUS=ALAYV CH, 1, DAVAL I) 
DLi=FLUAT (DAVAL~2043) 
Dui=F LUAT CUAVAL I LO4d ) 


LM DEBE or 


EUSUM=0. 941 /508F0D5UM + .O29 12908 (FDUUT + Lie) 


ViF=biebi 

Vites le DSU 

THETA. G1LOBGDYZDUFV I 

IHDUI EC PHE LAS THU J eRALE + US Cet Ati) 
VILE-O. FOI SGODURVESL -. OUOLbO4L.. 
PFHILETHIE tA + VL 
PHIDUTE CPHIOP doe Rede UY Cee HATED 
U=RE REXEL CE VERAL DOL 

TREU tic. dou 

PL=IR/ Ger PR eb) 

FLUTTER ATE X (FLL ID 

Wiss DMN PHU) +H M ER Lt OV DE ePLDU TO / 04. ODOFHE 
PURSE 


lL; this sertimn 16 used to install a celling on the value of 


Il current which can seen by actuator cantroller. 
LECRD 2k. bon Ole 2 ae ite 
1=4.Q0D-OQ%4 
Pleo 


ENDIF 
lec i ell. +.0D-03) L=4. Ups 
[Pat 2 lols —4. Os) =—4., OD-OS 
Voe=omOO# 1 
YENI C2. O46 DU0S4V G7 10D0 ee Orie 
SIAITUS=ALDVCLO,Y) 
DEC ee te Miers 
JS Jit l 
PAC JJ =RR I 
VAC Js SVL 
tACIJ2I= THETA 
* FUR C22 =i SUM 
JI=Jits 


74 


1) 


END LF 

FL 2 =FL 

J=J+1 

pad c= rl 
[HUOS= hk 1A 
FOUUT=D. 

WE MO Peete tO Sigel OD 
Y=/04% 
SI'AIUS=ALDV (LU, Y) 
SIAIUS=AL Titel ¢ 2 
DU 2O J1L=1, 400 


Cee ies so) Clie, OOO, FAL IIIs 1atJi), VAIL? 


FURIMALCIX, $¢2X,F10.4)) 
LUNI ENUE 
END 


15 


APPENDIX C 


TRAJECTORY CONTROL PROGRAM 
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L TRAJ.FUR 15 A SAMFLE FRUGRAM USED DURING TKAJEL I UY 
PeeeOIN A re. 


FNCU ES ALLE Ser Ur’ 
SINULUDE: 7 ALLERRS.F UR? 


ea eet lees Me tem toes cite) pik tine Pe. OE lerrt yt rit Ll 
/FD,V3,VALCIOOL), 
eee eet lirics tt FL elk FLITE, 
/1AC1001),FAC1001),FD1, 
Baan let ede Va ele, (HD, FDDU) C1001), 
PEvoul ior (LOOth) 1, DSuM 

REAL#+ FAIE,D1, D2, KR,KYV, HE. 

INT EIGER #2 ADISAINS (16), ADCHANC16), LLOUNF LInC 16), 
/BASEADE, LU DEVID, UDEVE LIS, SLAN, DAVAL, DAVAL, Y, 
/SIAIUS,FHLDD, DACLOOO) 

LUMMUN/SLONE Lis/ LLUNE Lis 

ELDIUIVALENLE DUN Ls thUBASEADE), KHADEADE), 
Jeera tive V LD) DEV IU) 

7 (GONE fis CR ICDEVF LAIsS?) , LDEVFLIS) , 

/CLLUN bin CRISLlaAN) , SLAND, CLLUUNE Lis CRLICHANNEL SS), CHAN) 
L ihe variables are defined in the simulation proaqram FIOM 
Ll. e@exrept as nated belmw. 

FA Ee =o. Q0O0 

L=0. Sb sOcuguu 

Pole 7 OSUGUW)/ 

Vireo. OGLE /D-O4 

Be=G'90.ODUG 

WMStee aa 7 LDH OS 

Pie so te 7/1 

Jat 4 

AD vihinedre) =). CHC) 

Ere =O, DODO 

L tpuUuUd 15 Aa Gummy variable usec 1m the didital filter. 

Pvite ==). DU 

Boe EO deb 
L joe 1S a dummy variable used to compute larqe angle 
li velority. 

FHV. OG 
POL=O. ODO 

HE=Bo/. YS 
BREE 4o. 760 / 

BV=z60. 0000) 

L FUSUM 15 the value ot the filtered small angle siqnal. 
rUSUM=O0,. ODO 

Pg 2==—0 061041 240 D0 

Y=eLsOotyd 

Lt FOI 15 the initial load pressure drop. 

PLIS2S. geSUSDO/ CDM#EFF 2 

Filbrw=e. OOUUEDAIANCL. OU) 

URIEMCUNL P=1lo,b tLe =? (He ein. VAL? SiAIUS="’ NEW?’ ) 

L INL LAL Ze tHe BUArD 

SIAILUS=ALINI IC) 
SIALUDS=ALoOrR! I) 


WEE 


SIAMWIS=ALSF CRAI KD 
SIAIUS=ALFSEI OC) 
STATUS=ALISL C TUNE Lis) 
SIAIUS=ALDVLG, Y) 


J1L=5 
J=Q 
i IDENTIFY THtk WUNIRUL FARAME TERS 
* Wet TEC#, *#) "ENFUT TRANSFER FUNCITLUN CHE?’ 
* READ, HE 
* Wiebe Ce, %#) TINFUT GALNS CKRF,EV) (CkrEAL)? 
* RE abe, RR, RY 
* WRITE C#,%) 'INFUI PHE DESITEED ARM FUSS) TONS heaves ee 
* READ#, FHIDDI1 
* Mh DN te i 
* PHiD=( (eR LEY / 1 BO) #FHIDD1) 
* FeINI#, PAID 
U=ODO 
J1l=1 
FO1L=OD0 


SL=LODO/ SS. O4F8D05 
i LUMMENCE FUSILILUNINis UF THE ARM 
Ow lt od whl. S009) PHIiD= (1.061027 ey 
[Ftd Ise. 400 . AND. J .Li. S900) PHED=2.000 
Jt (ld <i. GOO SAND Zein sogrs 
PHI D=(= 14 O07 ee OO ea een 
frets wb. LOU Sera boa. 
REE eR LD 
l- Begin data acquisition process. 
SiATUS=ALAVC1,1,DAVAL) 
SIAIUS=ALAVCY, 1,DAVALY) 
Di=F LUA CDAVAL~—“040) 
Dit=F LUA ODAVAL Zod 2 
FPDSUM=0. 941 /SU08FDSUM + . Oe ILS IOs CRFDUUL + Died 
ViFs1#D1 
Vers leer DSU 
THE IA=. GLOBG50YD0eVI 
THDUI= (THE TA IHD) eKAIE + U/CSRAIE? 
VES". 407 Sc0D048V270 =. VOloot7 
* VI=O. O 
PHIF tHE IA + VEL 
FHI DUI=CRFHI-FHi2d *RALE + U/C 2*RALE) 
U=Eh- Er eA ever Ale | 
PRL tio. ti 
te etek tiie) 
LDU IHRE AVES (r-rel? 
G=DMe1TAUO I-A IPL Y 1 HP 7 ee ere ee 
PO=hrS—FL 
(lL ihis Sectiom 16 Used CmernSt alse tacit ic ore 
losf current which can be seen by the actuator controller. 
Lie eC eee) eS) iia te 
b=4.0D0—-O4 


ELSE 
P= CU CHADS EIT CRD? STOOO,. OO 
eat IE |) 
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1) 


ot tee 
eee Aw) 


Age 


i tee te OOo p= 7p 
trod wLd. —4.0D-O3) Le-4.0D-O%5 
=) % 

YENI Ce. O46D0GRVEG/10D0 + BOds 
SIAIUS=ALDV(O, VY) 

trod 2b. J2) THEN 

=) er 1 

FACIL O=FHI 

VAC =Vi 

TACJ2Z I= THETA 

TUR CJ =F USUM 

DACIJYI=D2 

CL= Jes 

ENDIF 

PLISEL 

J=J+1 

PHi2=FH I 

THU THETA 

FOUUT=Dz 

IFCJ «Le. YOUU. GUTU Ob 
Y=2090 

SIAIUS=ALDVIO, ¥2 
SIATUS=ALTERM( ! 

Dee Ain ECL) 


WRITE CID, wb) C318. O250),FACI12, FACS19,VALIL) 


FURIMAPCIX, $02X,F 10.49) 
LUNE t NOt 
END 
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